/******************************************************************************
 *
 * MotionViewer
 * Copyright (C) 2007 Sangyoon Lee, Electronic Visualization Laboratory, 
 * University of Illinois at Chicago
 *
 * This software is free software; you can redistribute it and/or modify it 
 * under the terms of the GNU Lesser General Public License as published by
 * the Free Software Foundation; either Version 2.1 of the License, or 
 * (at your option) any later version.
 *
 * This software is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
 * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public 
 * License for more details.
 * 
 * You should have received a copy of the GNU Lesser Public License along
 * with this software; if not, write to the Free Software Foundation, Inc., 
 * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
 *
 * Questions or comments about MotionViewer should be directed to 
 * cavern@evl.uic.edu
 *
 *****************************************************************************/

#pragma warning(disable:4244)	
#pragma warning(disable:4018)	

#include <iostream>
#include <cassert>
#include <string>
#include <vector>
#include <algorithm>	
#include <functional>
#include <limits>

#include <math.h>
#include <ogreplatform.h>
#if OGRE_PLATFORM == OGRE_PLATFORM_APPLE
#include <Carbon/Carbon.h>
#endif

#include "ViconRTData.h"
#include "ViconSubject.h"


//-----------------------------------------------------------------------------
//	The recv call may return with a half-full buffer.
//	revieve keeps going until the buffer is actually full.
#ifdef _WIN32
bool recieve(SOCKET Socket, char * pBuffer, int BufferSize)
#else
bool recieve(int Socket, char * pBuffer, int BufferSize)
#endif
{
	char * p = pBuffer;
	char * e = pBuffer + BufferSize;

	int result;

	while(p != e)
	{
		result = recv(	Socket, p, e - p, 0 );

		if(result < 0)
			return false;
		
		p += result;
	}

	return true;
}


//	There are also some helpers to make the code a little less ugly.
#ifdef _WIN32
bool recieve(SOCKET Socket, long int & Val)
#else
bool recieve(int Socket, long int & Val)
#endif
{
	return recieve(Socket, (char*)&Val, sizeof(Val));
}

#ifdef _WIN32
bool recieve(SOCKET Socket, unsigned long int & Val)
#else
bool recieve(int Socket, unsigned long int & Val)
#endif
{
	return recieve(Socket, (char*) & Val, sizeof(Val));
}

#ifdef _WIN32
bool recieve(SOCKET Socket, double & Val)
#else
bool recieve(int Socket, double & Val)
#endif
{
	return recieve(Socket, (char*) & Val, sizeof(Val));
}


//-----------------------------------------------------------------------------
ViconRTData::ViconRTData()
{
	m_bConnected = false;
	m_bViconRT = false;
	m_fElapsedTime = 0;
	bufferSize = 2040;

	m_tType = MT_RTENGINE;

}

ViconRTData::~ViconRTData()
{
	// close socket
#ifdef _WIN32
	if(closesocket(SocketHandle) == SOCKET_ERROR)
#else
	if(close(SocketHandle) < 0)
#endif
	{
		std::cout << "Failed to close Socket" << std::endl;
	}

	// clean up data
	m_vInfo.clear();
	m_vMarkerChannels.clear();
	m_vBodyChannels.clear();
	m_vMarkerData.clear();
	m_vBodyData.clear();

}

//#ifdef _WIN32
bool ViconRTData::initSocket(char* addr)
{
	char serveraddr[32];

	//- Initialisation - - - - - - - - - - - - - - - - - - - - - - - - - - - -
#ifdef _WIN32
	//  Windows-specific initialisation.
	WORD wVersionRequested;
	WSADATA wsaData;
	wVersionRequested = MAKEWORD( 2, 0 ); 
	if(WSAStartup( wVersionRequested, &wsaData ) != 0)
	{
		std::cout << "Socket Initialization Error" << std::endl;
		return false;
	}
#endif	
	// let's simply make it connect to local host
	if (strlen(addr) < 1)
	{
		char ac[80];
		if (gethostname(ac, sizeof(ac)) == SOCKET_ERROR) {
			return false;
		}
		strcpy(serveraddr, ac);
	}
	else
		strcpy(serveraddr, addr);

	// Create Socket
	SocketHandle = INVALID_SOCKET;
	
	struct protoent*	pProtocolInfoEntry;
	char*				protocol;
	int					type;

	protocol = "tcp";
	type = SOCK_STREAM;

	pProtocolInfoEntry = getprotobyname(protocol);
	assert(pProtocolInfoEntry);

	if(pProtocolInfoEntry)
		SocketHandle = socket(PF_INET, type, pProtocolInfoEntry->p_proto);

	if(SocketHandle == INVALID_SOCKET)
	{
		std::cout << "Socket Creation Error" << std::endl;
		return false;
	}

	struct hostent*		pHostInfoEntry;
	struct sockaddr_in	Endpoint;

	static const int port = 800;

	memset(&Endpoint, 0, sizeof(Endpoint));
	Endpoint.sin_family	= AF_INET;
	Endpoint.sin_port	= htons(port);

	pHostInfoEntry = gethostbyname(serveraddr);

	if(pHostInfoEntry)
		memcpy(&Endpoint.sin_addr,	pHostInfoEntry->h_addr, 
										pHostInfoEntry->h_length);
	else
		Endpoint.sin_addr.s_addr = inet_addr(serveraddr);

	if(Endpoint.sin_addr.s_addr == INADDR_NONE)
	{
		std::cout << "Bad Address" << std::endl;
		return false;
	}

	//	Create Socket
	int result = connect(	SocketHandle, (struct sockaddr*) & Endpoint, sizeof(Endpoint));

	if(result == SOCKET_ERROR)
	{
		std::cout << "Failed to create Socket" << std::endl;
#ifdef _WIN32
		int e = WSAGetLastError();
		closesocket(SocketHandle);
#else
		close(SocketHandle);
#endif
		return false;
	}

	return true;
}
/*
#else
bool ViconRTData::initSocket(char* addr)
{
	char serveraddr[32];
	
	//- Initialisation - - - - - - - - - - - - - - - - - - - - - - - - - - - 
	// let's simply make it connect to local host if addr is not specified
	if (strlen(addr) < 1)
	{
		char ac[80];
		if (gethostname(ac, sizeof(ac)) < 0) {
			return false;
		}
		strcpy(serveraddr, ac);
	}
	else
		strcpy(serveraddr, addr);
	
	// Create Socket
	SocketHandle = -1;
	
	struct protoent*	pProtocolInfoEntry;
	char*				protocol;
	int					type;
	
	protocol = "tcp";
	type = SOCK_STREAM;
	
	pProtocolInfoEntry = getprotobyname(protocol);
	assert(pProtocolInfoEntry);
	
	if(pProtocolInfoEntry)
		SocketHandle = socket(PF_INET, type, pProtocolInfoEntry->p_proto);
	
	if(SocketHandle < 0)
	{
		std::cout << "Socket Creation Error" << std::endl;
		return false;
	}
	
	struct hostent*		pHostInfoEntry;
	struct sockaddr_in	Endpoint;
	
	static const int port = 800;
	
	memset(&Endpoint, 0, sizeof(Endpoint));
	Endpoint.sin_family	= AF_INET;
	Endpoint.sin_port	= htons(port);
	
	pHostInfoEntry = gethostbyname(serveraddr);
	
	if(pHostInfoEntry)
		memcpy(&Endpoint.sin_addr,	pHostInfoEntry->h_addr, 
			   pHostInfoEntry->h_length);
	else
		Endpoint.sin_addr.s_addr = inet_addr(serveraddr);
	
	if(Endpoint.sin_addr.s_addr == INADDR_NONE)
	{
		std::cout << "Bad Address" << std::endl;
		return false;
	}
	
	//	Create Socket
	int result = connect(	SocketHandle, (struct sockaddr*) & Endpoint, sizeof(Endpoint));
	
	if(result < 0)
	{
		std::cout << "Failed to create Socket" << std::endl;
		close(SocketHandle);
		return false;
	}

	return true;
}
#endif
*/
bool ViconRTData::connectToEngine()
{
	
	//	A connection with the Vicon Realtime system is now open.
	//	The following section implements the new Computer Graphics Client interface.
	try
	{
		//- Get Info - - - - - - - - - - - - - - - - - - - - - - - - - - - -
		//	Request the channel information
		pBuff = buff;

		* ((long int *) pBuff) = ClientCodes::EInfo;
		pBuff += sizeof(long int);
		* ((long int *) pBuff) = ClientCodes::ERequest;
		pBuff += sizeof(long int);

		if(send(SocketHandle, buff, pBuff - buff, 0) == SOCKET_ERROR)
			throw std::string("Error Requesting");

		long int packet;
		long int type;

		if(!recieve(SocketHandle, packet))
			throw std::string("Error Recieving");

		if(!recieve(SocketHandle, type))
			throw std::string("Error Recieving");
		
		if(type != ClientCodes::EReply)
			throw std::string("Bad Packet");

		if(packet != ClientCodes::EInfo)
			throw std::string("Bad Reply Type");
			
		//long int size;

		if(!recieve(SocketHandle, size))
			throw std::string();

		m_vInfo.resize(size);

		std::vector< std::string >::iterator iInfo;

		for(iInfo = m_vInfo.begin(); iInfo != m_vInfo.end(); iInfo++)
		{
			long int s;
			char c[255];
			char * p = c;

			if(!recieve(SocketHandle, s)) 
				throw std::string();

			if(!recieve(SocketHandle, c, s)) 
				throw std::string();
			
			p += s;
			
			*p = 0;
			
			*iInfo = std::string(c);
		}

		//- Parse Info - - - - - - - - - - - - - - - - - - - - - - - - - - - -
		//	The info packets now contain the channel names.
		//	Identify the channels with the various dof's.
		
		for(iInfo = m_vInfo.begin(); iInfo != m_vInfo.end(); iInfo++)
		{
			//	Extract the channel type
			int openBrace = iInfo->find('<');
			
			if(openBrace == iInfo->npos) 
				throw std::string("Bad Channel Id");
		
			int closeBrace = iInfo->find('>');
			
			if(closeBrace == iInfo->npos) 
				throw std::string("Bad Channel Id");

			closeBrace++;

			std::string Type = iInfo->substr(openBrace, closeBrace-openBrace);

			//	Extract the Name

			std::string Name = iInfo->substr(0, openBrace);

			int space = Name.rfind(' ');
			
			if(space != Name.npos) 
				Name.resize(space);

			std::vector< MarkerChannel >::iterator iMarker;
			std::vector< BodyChannel >::iterator iBody;
			std::vector< std::string >::const_iterator iTypes;

			iMarker = std::find(	m_vMarkerChannels.begin(), 
									m_vMarkerChannels.end(), Name);

			iBody = std::find(m_vBodyChannels.begin(), m_vBodyChannels.end(), Name);

			if(iMarker != m_vMarkerChannels.end())
			{
				//	The channel is for a marker we already have.
				iTypes = std::find(	ClientCodes::MarkerTokens.begin(), ClientCodes::MarkerTokens.end(), Type);
				if(iTypes != ClientCodes::MarkerTokens.end())
					iMarker->operator[](iTypes - ClientCodes::MarkerTokens.begin()) = iInfo - m_vInfo.begin();
			}
			else 
			if(iBody != m_vBodyChannels.end())
			{
				//	The channel is for a body we already have.
				iTypes = std::find(ClientCodes::BodyTokens.begin(), ClientCodes::BodyTokens.end(), Type);
				if(iTypes != ClientCodes::BodyTokens.end())
					iBody->operator[](iTypes - ClientCodes::BodyTokens.begin()) = iInfo - m_vInfo.begin();
			}
			else
			if((iTypes = std::find(ClientCodes::MarkerTokens.begin(), ClientCodes::MarkerTokens.end(), Type))
					!= ClientCodes::MarkerTokens.end())
			{
				//	Its a new marker.
				m_vMarkerChannels.push_back(MarkerChannel(Name));
				m_vMarkerChannels.back()[iTypes - ClientCodes::MarkerTokens.begin()] = iInfo - m_vInfo.begin();
			}
			else
			if((iTypes = std::find(ClientCodes::BodyTokens.begin(), ClientCodes::BodyTokens.end(), Type))
					!= ClientCodes::BodyTokens.end())
			{
				//	Its a new body.
				m_vBodyChannels.push_back(BodyChannel(Name));
				m_vBodyChannels.back()[iTypes - ClientCodes::BodyTokens.begin()] = iInfo - m_vInfo.begin();
			}
			else
			if(Type == "<F>")
			{
				FrameChannel = iInfo - m_vInfo.begin();
			}
			else
			{
				//	It could be a new channel type.
			}

		}
	}
	catch(const std::string & rMsg)
	{
		if(rMsg.empty())
			std::cout << "Error! Error! Error! Error! Error!" << std::endl;
		else
			std::cout << rMsg.c_str() << std::endl;

		return false;
	}

	m_vMarkerData.resize(m_vMarkerChannels.size());
	m_vBodyData.resize(m_vBodyChannels.size());

	return true;
}

bool ViconRTData::connectRT(char* addr)
{
	if (m_bConnected)
		disconnectRT();

	m_bConnected = false;

	if (initSocket(addr))
		if (connectToEngine())
			m_bConnected = true;
		else
			disconnectRT();

	m_fElapsedTime = 0.0f;

	m_sSource = std::string(addr);

	return m_bConnected;
}

bool ViconRTData::disconnectRT()
{
	// close socket
#ifdef _WIN32
	if(closesocket(SocketHandle) == SOCKET_ERROR)
#else
	if(close(SocketHandle) == SOCKET_ERROR)
#endif
	{
		std::cout << "Failed to close Socket" << std::endl;
		return false;
	}
	
	// clean up data
	m_vInfo.clear();
	m_vMarkerChannels.clear();
	m_vBodyChannels.clear();
	m_vMarkerData.clear();
	m_vBodyData.clear();

	// reset vars
	m_fElapsedTime = 0.0f;
	m_bConnected = false;

	return true;
}


void ViconRTData::update(float addedTime)
{
	if (!m_bConnected)
		return;

	m_fElapsedTime += addedTime;
	if (m_fElapsedTime < 1.0f / 60.0f)
		return;
	else
		m_fElapsedTime = 0;
	
	//- Get Data - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 
	//	Get the data using the request/reply protocol.

	std::vector< double > data;
	data.resize(m_vInfo.size());

	double timestamp;

	{
		pBuff = buff;

		* ((long int *) pBuff) = ClientCodes::EData;
		pBuff += sizeof(long int);
		* ((long int *) pBuff) = ClientCodes::ERequest;
		pBuff += sizeof(long int);

		if(send(SocketHandle, buff, pBuff - buff, 0) == SOCKET_ERROR)
			return;

		long int packet;
		long int type;

		//	Get and check the packet header.
		if(!recieve(SocketHandle, packet))
			return;

		if(!recieve(SocketHandle, type))
			return;

		if(type != ClientCodes::EReply)
			return;

		if(packet != ClientCodes::EData)
			return;

		if(!recieve(SocketHandle, size))
			return;

		if(size != m_vInfo.size())
			return;

		//	Get the data.
		std::vector< double >::iterator iData;

		for(iData = data.begin(); iData != data.end(); iData++)
		{	
			if(!recieve(SocketHandle, *iData)) 
				return;
		}

		//- Look Up Channels - - - - - - - - - - - - - - - - - - - - - - - 
		//  Get the TimeStamp
		timestamp = data[FrameChannel];

		//	Get the channels corresponding to the markers.
		//	Y is up
		//	The values are in millimeters
		std::vector< MarkerChannel >::iterator iMarker;
		std::vector< MarkerData >::iterator iMarkerData;

		for(	iMarker = m_vMarkerChannels.begin(), 
				iMarkerData = m_vMarkerData.begin(); 
				iMarker != m_vMarkerChannels.end(); iMarker++, iMarkerData++)
		{
			if (iMarker->X != -1)
			{
				iMarkerData->X = -data[iMarker->X];
				iMarkerData->Z = data[iMarker->Y];
				iMarkerData->Y = data[iMarker->Z];
			}

			//if (iMarker->O != -1)
			//	int kk = 0;
			//if(data[iMarker->O] > 0.5)
			//	iMarkerData->Visible = false;
			//else
			//	iMarkerData->Visible = true;
		}

		//	Get the channels corresponding to the bodies.
		//=================================================================
		//	The bodies are in global space
		//	The world is Z-up
		//	The translational values are in millimeters
		//	The rotational values are in radians
		//=================================================================
		std::vector< BodyChannel >::iterator iBody;
		std::vector< BodyData >::iterator iBodyData;

		for(	iBody = m_vBodyChannels.begin(), 
				iBodyData = m_vBodyData.begin(); 
				iBody != m_vBodyChannels.end(); iBody++, iBodyData++)
		{

			if (iBody->TX !=-1)
			{
				iBodyData->TX = -data[iBody->TX];
				iBodyData->TZ = data[iBody->TY];
				iBodyData->TY = data[iBody->TZ];
			}
			if (iBody->tX != -1)
			{
				iBodyData->tX = -data[iBody->tX];
				iBodyData->tZ = data[iBody->tY];
				iBodyData->tY = data[iBody->tZ];
			}

			//	The channel data is in the angle-axis form.
			//	The following converts this to a quaternion.
			//=============================================================
			//	An angle-axis is vector, the direction of which is the axis
			//	of rotation and the length of which is the amount of 
			//	rotation in radians. 
			//=============================================================
			double len, tmp;
			
			if (iBody->RX != -1)
			{
				len = sqrt(	data[iBody->RX] * data[iBody->RX] + 
							data[iBody->RY] * data[iBody->RY] + 
							data[iBody->RZ] * data[iBody->RZ]);

				iBodyData->QW = cos(len / 2.0);
				tmp = sin(len / 2.0);
				if (len < 1e-10) 
				{
					iBodyData->QX = -data[iBody->RX];
					iBodyData->QZ = data[iBody->RY];
					iBodyData->QY = data[iBody->RZ];
				} 
				else 
				{
					iBodyData->QX = -data[iBody->RX] * tmp/len;
					iBodyData->QZ = data[iBody->RY] * tmp/len;
					iBodyData->QY = data[iBody->RZ] * tmp/len;
				}
				
				//	The following converts angle-axis to a rotation matrix.
				double c, s, x, y, z;
				c=s=x=y=z=0;

				if (len < 1e-15)
				{
					iBodyData->GlobalRotation[0][0] = iBodyData->GlobalRotation[1][1] = iBodyData->GlobalRotation[2][2] = 1.0;
					iBodyData->GlobalRotation[0][1] = iBodyData->GlobalRotation[0][2] = iBodyData->GlobalRotation[1][0] =
					iBodyData->GlobalRotation[1][2]	= iBodyData->GlobalRotation[2][0] = iBodyData->GlobalRotation[2][1] = 0.0;
				}
				else
				{
					x = -data[iBody->RX]/len;
					z = data[iBody->RY]/len;
					y = data[iBody->RZ]/len;

					c = cos(len);
					s = sin(len);

					iBodyData->GlobalRotation[0][0] = c + (1-c)*x*x;
					iBodyData->GlobalRotation[0][1] =     (1-c)*x*y + s*(-z);
					iBodyData->GlobalRotation[0][2] =     (1-c)*x*z + s*y;
					iBodyData->GlobalRotation[1][0] =     (1-c)*y*x + s*z;
					iBodyData->GlobalRotation[1][1] = c + (1-c)*y*y;
					iBodyData->GlobalRotation[1][2] =     (1-c)*y*z + s*(-x);
					iBodyData->GlobalRotation[2][0] =     (1-c)*z*x + s*(-y);
					iBodyData->GlobalRotation[2][1] =     (1-c)*z*y + s*x;
					iBodyData->GlobalRotation[2][2] = c + (1-c)*z*z;
				}

				// now convert rotation matrix to nasty Euler angles (yuk)
				// you could convert direct from angle-axis to Euler if you wish

				//	'Look out for angle-flips, Paul...'
				//  Algorithm: GraphicsGems II - Matrix Techniques VII.1 p 320
				assert(fabs(iBodyData->GlobalRotation[0][2]) <= 1);
				iBodyData->EulerY = asin(-iBodyData->GlobalRotation[2][0]);

				if(fabs(cos(y)) > 
					std::numeric_limits<double>::epsilon() ) 	// cos(y) != 0 Gimbal-Lock
				{
					iBodyData->EulerX = atan2(iBodyData->GlobalRotation[2][1], iBodyData->GlobalRotation[2][2]);
					iBodyData->EulerZ = atan2(iBodyData->GlobalRotation[1][0], iBodyData->GlobalRotation[0][0]);
				}
				else
				{
					iBodyData->EulerZ = 0;
					iBodyData->EulerX = atan2(iBodyData->GlobalRotation[0][1], iBodyData->GlobalRotation[1][1]);
				}

			}
			
			// this is really for vicon RT connection (local rotation)
			// sdk emulator showed different label before... so adding one more here
			if (iBody->baX != -1)
			{
				m_bViconRT = true;
				
				len = sqrt(	data[iBody->baX] * data[iBody->baX] + 
						   data[iBody->baY] * data[iBody->baY] + 
						   data[iBody->baZ] * data[iBody->baZ]);
				
				iBodyData->bqW = cos(len / 2.0);
				tmp = sin(len / 2.0);
				if (len < 1e-10) 
				{
					iBodyData->bqX = -data[iBody->baX];
					iBodyData->bqZ = data[iBody->baY];
					iBodyData->bqY = data[iBody->baZ];
				} 
				else 
				{
					iBodyData->bqX = -data[iBody->baX] * tmp/len;
					iBodyData->bqZ = data[iBody->baY] * tmp/len;
					iBodyData->bqY = data[iBody->baZ] * tmp/len;
				}
				
				//	The following converts angle-axis to a rotation matrix.
				double c, s, x, y, z;
				c=s=x=y=z=0;
				
				if (len < 1e-15)
				{
					iBodyData->LocalRotationRT[0][0] = iBodyData->LocalRotationRT[1][1] = iBodyData->LocalRotationRT[2][2] = 1.0;
					iBodyData->LocalRotationRT[0][1] = iBodyData->LocalRotationRT[0][2] = iBodyData->LocalRotationRT[1][0] =
					iBodyData->LocalRotationRT[1][2]	= iBodyData->LocalRotationRT[2][0] = iBodyData->LocalRotationRT[2][1] = 0.0;
				}
				else
				{
					x = -data[iBody->baX]/len;
					z = data[iBody->baY]/len;
					y = data[iBody->baZ]/len;
					
					c = cos(len);
					s = sin(len);
					
					iBodyData->LocalRotationRT[0][0] = c + (1-c)*x*x;
					iBodyData->LocalRotationRT[0][1] =     (1-c)*x*y + s*(-z);
					iBodyData->LocalRotationRT[0][2] =     (1-c)*x*z + s*y;
					iBodyData->LocalRotationRT[1][0] =     (1-c)*y*x + s*z;
					iBodyData->LocalRotationRT[1][1] = c + (1-c)*y*y;
					iBodyData->LocalRotationRT[1][2] =     (1-c)*y*z + s*(-x);
					iBodyData->LocalRotationRT[2][0] =     (1-c)*z*x + s*(-y);
					iBodyData->LocalRotationRT[2][1] =     (1-c)*z*y + s*x;
					iBodyData->LocalRotationRT[2][2] = c + (1-c)*z*z;
				}
				
				// now convert rotation matrix to nasty Euler angles (yuk)
				// you could convert direct from angle-axis to Euler if you wish
				
				//	'Look out for angle-flips, Paul...'
				//  Algorithm: GraphicsGems II - Matrix Techniques VII.1 p 320
				assert(fabs(iBodyData->LocalRotationRT[0][2]) <= 1);
				iBodyData->beulerY = asin(-iBodyData->LocalRotationRT[2][0]);
				
				if(fabs(cos(y)) > 
				   std::numeric_limits<double>::epsilon() ) 	// cos(y) != 0 Gimbal-Lock
				{
					iBodyData->beulerX = atan2(iBodyData->LocalRotationRT[2][1], iBodyData->LocalRotationRT[2][2]);
					iBodyData->beulerZ = atan2(iBodyData->LocalRotationRT[1][0], iBodyData->LocalRotationRT[0][0]);
				}
				else
				{
					iBodyData->beulerZ = 0;
					iBodyData->beulerX = atan2(iBodyData->LocalRotationRT[0][1], iBodyData->LocalRotationRT[1][1]);
				}
				
			}
			
			
			// this works with sdk emulator
			if (iBody->rX != -1)
			{
				len = sqrt(	data[iBody->rX] * data[iBody->rX] + 
							data[iBody->rY] * data[iBody->rY] + 
							data[iBody->rZ] * data[iBody->rZ]);

				iBodyData->qW = cos(len / 2.0);
				tmp = sin(len / 2.0);
				if (len < 1e-10) 
				{
					iBodyData->qX = -data[iBody->rX];
					iBodyData->qZ = data[iBody->rY];
					iBodyData->qY = data[iBody->rZ];
				} 
				else 
				{
					iBodyData->qX = -data[iBody->rX] * tmp/len;
					iBodyData->qZ = data[iBody->rY] * tmp/len;
					iBodyData->qY = data[iBody->rZ] * tmp/len;
				}
				
				//	The following converts angle-axis to a rotation matrix.
				double c, s, x, y, z;
				c=s=x=y=z=0;

				if (len < 1e-15)
				{
					iBodyData->LocalRotation[0][0] = iBodyData->LocalRotation[1][1] = iBodyData->LocalRotation[2][2] = 1.0;
					iBodyData->LocalRotation[0][1] = iBodyData->LocalRotation[0][2] = iBodyData->LocalRotation[1][0] =
					iBodyData->LocalRotation[1][2]	= iBodyData->LocalRotation[2][0] = iBodyData->LocalRotation[2][1] = 0.0;
				}
				else
				{
					x = -data[iBody->rX]/len;
					z = data[iBody->rY]/len;
					y = data[iBody->rZ]/len;

					c = cos(len);
					s = sin(len);

					iBodyData->LocalRotation[0][0] = c + (1-c)*x*x;
					iBodyData->LocalRotation[0][1] =     (1-c)*x*y + s*(-z);
					iBodyData->LocalRotation[0][2] =     (1-c)*x*z + s*y;
					iBodyData->LocalRotation[1][0] =     (1-c)*y*x + s*z;
					iBodyData->LocalRotation[1][1] = c + (1-c)*y*y;
					iBodyData->LocalRotation[1][2] =     (1-c)*y*z + s*(-x);
					iBodyData->LocalRotation[2][0] =     (1-c)*z*x + s*(-y);
					iBodyData->LocalRotation[2][1] =     (1-c)*z*y + s*x;
					iBodyData->LocalRotation[2][2] = c + (1-c)*z*z;
				}

				// now convert rotation matrix to nasty Euler angles (yuk)
				// you could convert direct from angle-axis to Euler if you wish

				//	'Look out for angle-flips, Paul...'
				//  Algorithm: GraphicsGems II - Matrix Techniques VII.1 p 320
				assert(fabs(iBodyData->LocalRotation[0][2]) <= 1);
				iBodyData->eulerY = asin(-iBodyData->LocalRotation[2][0]);

				if(fabs(cos(y)) > 
					std::numeric_limits<double>::epsilon() ) 	// cos(y) != 0 Gimbal-Lock
				{
					iBodyData->eulerX = atan2(iBodyData->LocalRotation[2][1], iBodyData->LocalRotation[2][2]);
					iBodyData->eulerZ = atan2(iBodyData->LocalRotation[1][0], iBodyData->LocalRotation[0][0]);
				}
				else
				{
					iBodyData->eulerZ = 0;
					iBodyData->eulerX = atan2(iBodyData->LocalRotation[0][1], iBodyData->LocalRotation[1][1]);
				}

			}
			
		}
	}
	
	if (m_bDebug)
		updateDebug();
}

void ViconRTData::updateDebug()
{
	// update debug body info
	m_sDebugBodyInfo = "";
	char istr[256];
	std::string label;
	label  = "=========================================================================\n";
	label += " Body Name     GLOBAL:  xpos      ypos      zpos     xrot   yrot   zrot\n";
	label += "=========================================================================\n";
	m_sDebugBodyInfo += label;
	int numBody = m_vBodyChannels.size();
	for (int i=0; i<numBody; i++)
	{
		sprintf(istr, "%-20s %8.2f  %8.2f  %8.2f   %5.2f  %5.2f  %5.2f \n",	
					m_vBodyChannels[i].Name.c_str(),
					-m_vBodyData[i].TX / 10.0f,
					 m_vBodyData[i].TZ / 10.0f,
					 m_vBodyData[i].TY / 10.0f,
					-m_vBodyData[i].EulerX,
					 m_vBodyData[i].EulerZ,
					 m_vBodyData[i].EulerY );

		m_sDebugBodyInfo += istr;
	}
	
	// update debug marker info
}

void ViconRTData::getMarkerPosition(int id, float* pos)
{
	if (id < 0 || id > m_vMarkerChannels.size() - 1)
	{
		pos[0] = pos[1] = pos[2] = 0.0f;
		return;
	}

	// unit is mm. so divide it by 10 to make cm scale
	pos[0] = m_vMarkerData[id].X / 10.0f;
	pos[1] = m_vMarkerData[id].Y / 10.0f;
	pos[2] = m_vMarkerData[id].Z / 10.0f;

	//vtxVicon2Ogre(pos);

}

void ViconRTData::getBodyTranslation(int id, float* trans, bool global)
{
	if (id < 0 || id > m_vBodyChannels.size() - 1)
	{
		trans[0] = trans[1] = trans[2] = 0.0f;
		return;
	}

	// unit is mm. so divide it by 10 to make cm scale
	if (global)
	{
		trans[0] = m_vBodyData[id].TX / 10.0f;
		trans[1] = m_vBodyData[id].TY / 10.0f;
		trans[2] = m_vBodyData[id].TZ / 10.0f;
	}
	else
	{
		trans[0] = m_vBodyData[id].tX / 10.0f;
		trans[1] = m_vBodyData[id].tY / 10.0f;
		trans[2] = m_vBodyData[id].tZ / 10.0f;
	}

	//vtxVicon2Ogre(trans);
}

void ViconRTData::getBodyRotation(int id, float* rot, bool global)
{
	if (id < 0 || id > m_vBodyChannels.size() - 1)
	{
		rot[0] = rot[1] = rot[2] = 0.0f;
		return;
	}

	if (global)
	{
		rot[0] = m_vBodyData[id].EulerX;
		rot[1] = m_vBodyData[id].EulerY;
		rot[2] = m_vBodyData[id].EulerZ;
	}
	else
	{
		if (m_bViconRT)
		{
			rot[0] = m_vBodyData[id].beulerX;
			rot[1] = m_vBodyData[id].beulerY;
			rot[2] = m_vBodyData[id].beulerZ;
		}
		else
		{
			rot[0] = m_vBodyData[id].eulerX;
			rot[1] = m_vBodyData[id].eulerY;
			rot[2] = m_vBodyData[id].eulerZ;
			
		}
		m_pSubject->computeRotation(id, rot);
	}

	//rotVicon2Ogre(rot);
}

void ViconRTData::getBodyQuaternion(int id, float* qt, bool global)
{
	if (id < 0 || id > m_vBodyChannels.size() - 1)
	{
		qt[0] = 1.0f;
		qt[1] = qt[2] = qt[3] = 0.0f;
		return;
	}

	if (global)
	{
		qt[0] = m_vBodyData[id].QW;
		qt[1] = m_vBodyData[id].QX;
		qt[2] = m_vBodyData[id].QY;
		qt[3] = m_vBodyData[id].QZ;
	}
	else
	{
		if (m_bViconRT)
		{
			qt[0] = m_vBodyData[id].bqW;
			qt[1] = m_vBodyData[id].bqX;
			qt[2] = m_vBodyData[id].bqY;
			qt[3] = m_vBodyData[id].bqZ;
		}
		else
		{
			qt[0] = m_vBodyData[id].qW;
			qt[1] = m_vBodyData[id].qX;
			qt[2] = m_vBodyData[id].qY;
			qt[3] = m_vBodyData[id].qZ;			
		}
		
		m_pSubject->computeQuaternion(id, qt);
	}

	//qtVicon2Ogre(qt);
}

